
// 模仿订阅的位置


#include "rclcpp/rclcpp.hpp"
#include <std_msgs/msg/float32_multi_array.hpp>
#include <iostream>
#include <chrono>
#include <string>

using namespace std;
using namespace std::chrono_literals; // 引入std::chrono_literals
// 使用用户提供的命名规则
float x_right = 2346.0f;  
float x_left = -2346.0f; 
float z_high = 2170.0f;

class JointPublisher : public rclcpp::Node
{
public:
    JointPublisher()
        : Node("joint_publisher_node")
    {
        // 发布者
        goat_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("/float_array_topic", 10);
        // 创建定时器，每100毫秒调用一次publish_messages方法
        timer_ = this->create_wall_timer(
            1000ms, // 定时器间隔（100毫秒）
            std::bind(&JointPublisher::publish_messages, this));
    }

private:
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr goat_pub_;
    rclcpp::TimerBase::SharedPtr timer_;

    void publish_messages()
    {
        // 创建Float32MultiArray消息
        auto message = std_msgs::msg::Float32MultiArray();

        // 填充消息数据
        message.data.push_back(x_right); 
        message.data.push_back(x_left);  
        message.data.push_back(z_high);  

        // 发布消息
        goat_pub_->publish(message);

        // 打印日志信息
        RCLCPP_INFO(this->get_logger(), "Published message: [x_right: %f, x_left: %f, z_high: %f]",
                    message.data[0], message.data[1], message.data[2]);
    }
};

int main(int argc, char **argv)
{
    // 初始化ROS节点
    rclcpp::init(argc, argv);
    // 创建发布者节点
    auto node = std::make_shared<JointPublisher>();
    // 启动ROS通信循环
    rclcpp::spin(node);
    // 清理资源
    rclcpp::shutdown();

    return 0;
}